﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;


using sicklrf = Microsoft.Robotics.Services.Sensors.SickLRF.Proxy;

namespace UrbanServices
{
    public partial class LRFForm : Form
    {


        UrbanLRF.UrbanLRFOperations _mainPort;
        public LRFForm(UrbanLRF.UrbanLRFOperations mainport)
        {

            InitializeComponent();
            _mainPort = mainport;
        }

        DateTime _lastLaser = DateTime.Now;

        public void ReplaceLaserData(sicklrf.State stateType)
        {
            if (stateType.TimeStamp < _lastLaser)
            {
                return;
            }
            _lastLaser = stateType.TimeStamp;
            //TimeSpan delay = DateTime.Now - stateType.TimeStamp;
            //lblDelay.Text = delay.ToString();

            Bitmap bmp = new Bitmap(stateType.DistanceMeasurements.Length, 100);
            Graphics g = Graphics.FromImage(bmp);
            g.Clear(Color.LightGray);

            int half = bmp.Height / 2;

            for (int x = 0; x < stateType.DistanceMeasurements.Length; x++)
            {
                int range = stateType.DistanceMeasurements[x];
                if (range > 0 && range < 8192)
                {
                    int h = bmp.Height * 500 / stateType.DistanceMeasurements[x];
                    if (h < 0)
                    {
                        h = 0;
                    }
                    Color col = LinearColor(Color.DarkBlue, Color.LightGray, 0, 8192, range);
                    g.DrawLine(new Pen(col), bmp.Width - x, half - h, bmp.Width - x, half + h);
                }
            }
            picLRF.Image = bmp;

            //if (btnConnectLRF.Enabled)
            //{
            //    btnConnectLRF.Enabled = false;
            //}
            //if (!btnDisconnect.Enabled)
            //{
            //    btnDisconnect.Enabled = true;
            //}
        }

        private Color LinearColor(Color nearColor, Color farColor, int nearLimit, int farLimit, int value)
        {
            if (value <= nearLimit)
            {
                return nearColor;
            }
            else if (value >= farLimit)
            {
                return farColor;
            }

            int span = farLimit - nearLimit;
            int pos = value - nearLimit;

            int r = (nearColor.R * (span - pos) + farColor.R * pos) / span;
            int g = (nearColor.G * (span - pos) + farColor.G * pos) / span;
            int b = (nearColor.B * (span - pos) + farColor.B * pos) / span;

            return Color.FromArgb(r, g, b);
        }

    }
}
